from car_controller import UWBCarController
import time
import math


def run_speed_test(controller, duration):
    """执行直行测试并返回移动数据"""
    start_x, start_y = controller._get_current_coordinates()
    start_time = time.perf_counter()
    print("start:x:{},y{}".format(start_x,start_y))
    # 执行直行
    controller._execute_timed_straight(duration)

    # 获取结束数据
    end_x, end_y = controller._get_current_coordinates()
    print("end:x:{},y:{}".format(end_x,end_y))
    actual_duration = time.perf_counter() - start_time
    distance = math.hypot(end_x - start_x, end_y - start_y)

    return {
        'start': (start_x, start_y),
        'end': (end_x, end_y),
        'duration': actual_duration,
        'distance': distance,
        'speed': distance / actual_duration,
        'direction': controller._calculate_direction(start_x, start_y, end_x, end_y)
    }


def run_turn_test(controller, turn_time, direction='left'):
    """执行转向测试并返回角度变化"""
    # 初始校准
#    start_angle = controller.calibrate_direction(1)

    # 执行转向
    if direction == 'left':
        controller._fixed_turn('left', turn_time)
    else:
        controller._fixed_turn('right', turn_time)

    # # 最终校准
    # end_angle = controller.calibrate_direction(1)
    #
    # return {
    #     'angle_change': (end_angle - start_angle) % 360,
    #     'turn_rate': (end_angle - start_angle) / turn_time
    # }


if __name__ == '__main__':
    car = UWBCarController(com_port="COM5", base_distance=4.8)

    try:
        while True:
            print("\n测试选项:")
            print("1. 直行速度测试")
            print("2. 转向性能测试")
            print("3. 退出")
            choice = input("请选择测试项目: ")

            if choice == '1':
                duration = float(input("输入测试时长(秒): "))
                result = run_speed_test(car, duration)
                print(f"\n直行 {duration:.1f}秒 结果:")
                print(f"实际时长: {result['duration']:.2f}秒")
                print(f"移动距离: {result['distance']:.2f}米")
                print(f"平均速度: {result['speed']:.2f} m/s")
                print(f"移动方向: {result['direction']:.1f}°")
                car.stop()
                time.sleep(1)  # 测试间隔

            elif choice == '2':
                turn_time = float(input("输入转向时间(秒): "))
                direction = input("转向方向(left/right): ").lower()
                result = run_turn_test(car, turn_time, direction)
                # print(f"\n转向测试结果:")
                # print(f"角度变化: {result['angle_change']:.1f}°")
                # print(f"转向速率: {result['turn_rate']:.1f}°/s")
                car.stop()
                time.sleep(1)  # 测试间隔

            elif choice == '3':
                break

            car.stop()
            time.sleep(1)  # 测试间隔

    finally:
        car.stop()
        time.sleep(1)  # 测试间隔